/*
 * Copyright (C) 2015-2017 Alibaba Group Holding Limited
 */
#include "aos/kernel.h"
#include "ulog/ulog.h"
#include "lidar.h"
#include "canbus.h"
#include "aos/cli.h"
#include "aos/hal/uart.h"
#include "modbus.h"
#include "canopen/io_board_api.h"
#include "car.h"

#define SYS_QUEUE_SIZE_MAX    (20)
#define SYS_START             (0)
#define SYS_READBY            (2)
#define SYS_WASH_ROOF_O       (3)
#define SYS_WASH_ROOF_C       (4)
#define SYS_WASH_SKIRT_SIDE_O (5)
#define SYS_WASH_SKIRT_SIDE_C (6)
#define SYS_WASH_SIDE_O       (7)
#define SYS_WASH_SIDE_C       (8)

#define MOTO_UP_BASE    (750) //1mm
#define MOTO_LIFT_BASE  (3889) //1°
#define ROOF_MAX  (800*MOTO_UP_BASE) //1100mm
//#define ROOF_MAX  (100*MOTO_UP_BASE) //1100mm
#define ROOF_LIFT_MAX  (350000)  //90°
#define SKIRT_H_MAX (150000) //200mm
#define SKIRT_L_MAX (19445) //5°

#define SYS_TASK_DEPTH (2048)

extern void panasonic_init();
extern void eta_init();
extern int sys_manager_init(void);
extern int car_ctrl_change(car_status_t ctrl);
extern void eta_opt(int val);
extern int liftting(int val);
extern int upping(int val);
void stop_up(void);
extern int brush(int v);
extern void lidar_opt(int v);


static aos_queue_t sys_info_queue;
static uint16_t sys_buf[SYS_QUEUE_SIZE_MAX];
static int wash_side_flag = 0;
static int step_debug = 0;

static int g_sys_status = SYS_START;
int roof_flag = 0;
void set_sys_status(int val)
{
	if (step_debug){
		return ;
	}
	g_sys_status = val;
	return ;
}

int get_sys_status(void)
{
	return g_sys_status;
}


int send_sys(uint16_t val)
{
	if (NULL == sys_info_queue.hdl){
		return -1;
	}
	aos_queue_send(&sys_info_queue, (void *)&val, sizeof(val));
}

void wash_side()
{
	if (!wash_side_flag && get_car_ctrl() == STANDBY){
		eta_opt(0);
		car_ctrl_change(ROUND);
		wash_side_flag = 1;
	}else if (wash_side_flag && get_car_ctrl() == STANDBY){
		eta_opt(2);
		wash_side_flag = 0;
	}else{
		car_ctrl_change(SEARCH);
	}	
}

static void sys_engine()
{
    int status;
	uint16_t aa = 0;
	uint32_t len = 0;
	float fa = 0.0;
/*	
	if (0 != aos_queue_new(&sys_info_queue, sys_buf, sizeof(sys_buf), SYS_QUEUE_SIZE_MAX)){
		printf("error:modbus queue new failed!\r\n");
		return ;
	}
*/
	//eta_opt(2);
	
    while (1) {

		if (get_car_ctrl() == STANDBY){
			switch(g_sys_status){
			case SYS_START:				
				break;
			case SYS_READBY:			
				break;
			case SYS_WASH_ROOF_O:
				gpio_output(1, 21, 1);
				lidar_opt(1);
				brush(2);
				aos_msleep(5000);
				liftting(-ROOF_LIFT_MAX);
				aos_msleep(8000);
				brush(0);
				eta_opt(0);
				car_ctrl_change(ROUND);
				aos_msleep(4000);
				//roof_flag = 1;
				break;
			case SYS_WASH_ROOF_C:
				roof_flag = 0;
				car_ctrl_change(SEARCH);
				//eta_opt(1);
				//r_ctrl_change(ROUND);
				break;
			case SYS_WASH_SKIRT_SIDE_O:
				liftting(-(ROOF_LIFT_MAX+SKIRT_L_MAX));
				//upping(-(ROOF_MAX-SKIRT_H_MAX));
				aos_msleep(300);
				car_ctrl_change(ROUND);
				break;
			case SYS_WASH_SKIRT_SIDE_C:
				liftting(SKIRT_L_MAX);
				upping(-SKIRT_H_MAX);
				aos_msleep(300);
				break;
			case SYS_WASH_SIDE_O:
				car_ctrl_change(ROUND);
				break;
			case SYS_WASH_SIDE_C:
				car_ctrl_change(SEARCH);
				break; 
			default:
				break;
				
			}
			set_sys_status(g_sys_status + 1);
		}
		aos_msleep(500);
    }
}

static void sys_cmd_routine(char *buf, int len, int argc, char **argv)
{
	int ret = 0;
	int arg1 = strtoul(argv[2], NULL, 10);
	int arg2 = strtoul(argv[3], NULL, 10);
	
	switch (*(argv[1]))
	{
	case 'c':
		g_sys_status = arg1;
		car_ctrl_change(STANDBY);
		aos_cli_printf("sys ctrl\r\n");
	   break;
	case 'o':
		car_ctrl_change(SEARCH);
		set_sys_status(SYS_READBY);
		break;
	case 's':
		step_debug = arg1;
		break;
	case 'p':
	   break;

	default:
	   aos_cli_printf("sys Unknown cs %d\r\n", *(argv[1]));
	   break;
	}
}


static struct cli_command sys_cmd[] = {
    { "sys","sys ctrl", sys_cmd_routine },
};


int sys_manager_init(void)
{
	int32_t rc = -1;
	rc = aos_task_new("moto_eta", sys_engine, NULL, SYS_TASK_DEPTH);
    if (0 != rc) {
		printf("moto eta creat fail\r\n");
	}
	aos_cli_register_commands(&sys_cmd[0], sizeof(sys_cmd) / sizeof(struct cli_command));
	return 0;
}

static int lidar_init()
{
	  init_lidar_ce30();
	  init_lidar_tfmini_plus();
	  return 0;
}

static void led()
{
	gpio_output(1, 2, 1);
	gpio_output(1, 3, 1);
	aos_msleep(500);
	gpio_output(1, 2, 0);
	gpio_output(1, 3, 0);
	aos_msleep(500);
}


int application_start(int argc, char *argv[])
{
    aos_set_log_level(LOG_INFO);

    if(0 != canbus_init()){
        LOGE("APP", "Start CAN Demo Failed");
    }
		
	//lwip_tcpip_init();
	modbus_init();
	//panasonic_init();
	eta_init();
	car_init();
	iomaster_init();
	aos_msleep(1000);
	panasonic_init();
	lidar_init();
	sys_manager_init();

	gpio_output(1, 3, 1);
	
    while (1) {
		car_ctrl();
		//car_go();
		//led();
		aos_msleep(250);	
    };
}

